/*
 * usr_math.c
 *
 *  Created on: Jan 9, 2025
 *      Author: Gangzhi.He
 */
#include "stdint-gcc.h"
#include "motor_drv_simu.h"
#include "stdio.h"
#include "math.h"
#include "stdbool.h"
#include "stdlib.h"

#define SIN_COS_TABLE {                                             \
    0x0000, 0x00C9, 0x0192, 0x025B, 0x0324, 0x03ED, 0x04B6, 0x057F, \
    0x0648, 0x0711, 0x07D9, 0x08A2, 0x096A, 0x0A33, 0x0AFB, 0x0BC4, \
    0x0C8C, 0x0D54, 0x0E1C, 0x0EE3, 0x0FAB, 0x1072, 0x113A, 0x1201, \
    0x12C8, 0x138F, 0x1455, 0x151C, 0x15E2, 0x16A8, 0x176E, 0x1833, \
    0x18F9, 0x19BE, 0x1A82, 0x1B47, 0x1C0B, 0x1CCF, 0x1D93, 0x1E57, \
    0x1F1A, 0x1FDD, 0x209F, 0x2161, 0x2223, 0x22E5, 0x23A6, 0x2467, \
    0x2528, 0x25E8, 0x26A8, 0x2767, 0x2826, 0x28E5, 0x29A3, 0x2A61, \
    0x2B1F, 0x2BDC, 0x2C99, 0x2D55, 0x2E11, 0x2ECC, 0x2F87, 0x3041, \
    0x30FB, 0x31B5, 0x326E, 0x3326, 0x33DF, 0x3496, 0x354D, 0x3604, \
    0x36BA, 0x376F, 0x3824, 0x38D9, 0x398C, 0x3A40, 0x3AF2, 0x3BA5, \
    0x3C56, 0x3D07, 0x3DB8, 0x3E68, 0x3F17, 0x3FC5, 0x4073, 0x4121, \
    0x41CE, 0x427A, 0x4325, 0x43D0, 0x447A, 0x4524, 0x45CD, 0x4675, \
    0x471C, 0x47C3, 0x4869, 0x490F, 0x49B4, 0x4A58, 0x4AFB, 0x4B9D, \
    0x4C3F, 0x4CE0, 0x4D81, 0x4E20, 0x4EBF, 0x4F5D, 0x4FFB, 0x5097, \
    0x5133, 0x51CE, 0x5268, 0x5302, 0x539B, 0x5432, 0x54C9, 0x5560, \
    0x55F5, 0x568A, 0x571D, 0x57B0, 0x5842, 0x58D3, 0x5964, 0x59F3, \
    0x5A82, 0x5B0F, 0x5B9C, 0x5C28, 0x5CB3, 0x5D3E, 0x5DC7, 0x5E4F, \
    0x5ED7, 0x5F5D, 0x5FE3, 0x6068, 0x60EB, 0x616E, 0x61F0, 0x6271, \
    0x62F1, 0x6370, 0x63EE, 0x646C, 0x64E8, 0x6563, 0x65DD, 0x6656, \
    0x66CF, 0x6746, 0x67BC, 0x6832, 0x68A6, 0x6919, 0x698B, 0x69FD, \
    0x6A6D, 0x6ADC, 0x6B4A, 0x6BB7, 0x6C23, 0x6C8E, 0x6CF8, 0x6D61, \
    0x6DC9, 0x6E30, 0x6E96, 0x6EFB, 0x6F5E, 0x6FC1, 0x7022, 0x7083, \
    0x70E2, 0x7140, 0x719D, 0x71F9, 0x7254, 0x72AE, 0x7307, 0x735E, \
    0x73B5, 0x740A, 0x745F, 0x74B2, 0x7504, 0x7555, 0x75A5, 0x75F3, \
    0x7641, 0x768D, 0x76D8, 0x7722, 0x776B, 0x77B3, 0x77FA, 0x783F, \
    0x7884, 0x78C7, 0x7909, 0x794A, 0x7989, 0x79C8, 0x7A05, 0x7A41, \
    0x7A7C, 0x7AB6, 0x7AEE, 0x7B26, 0x7B5C, 0x7B91, 0x7BC5, 0x7BF8, \
    0x7C29, 0x7C59, 0x7C88, 0x7CB6, 0x7CE3, 0x7D0E, 0x7D39, 0x7D62, \
    0x7D89, 0x7DB0, 0x7DD5, 0x7DFA, 0x7E1D, 0x7E3E, 0x7E5F, 0x7E7E, \
    0x7E9C, 0x7EB9, 0x7ED5, 0x7EEF, 0x7F09, 0x7F21, 0x7F37, 0x7F4D, \
    0x7F61, 0x7F74, 0x7F86, 0x7F97, 0x7FA6, 0x7FB4, 0x7FC1, 0x7FCD, \
    0x7FD8, 0x7FE1, 0x7FE9, 0x7FF0, 0x7FF5, 0x7FF9, 0x7FFD, 0x7FFE}

#define SIN_MASK 0x0300u
#define U0_90 0x0200u
#define U90_180 0x0300u
#define U180_270 0x0000u
#define U270_360 0x0100u

#define divSQRT_3     (int32_t)0x49E6 /* 1/sqrt(3) in q1.15 format=0.5773315*/
#define SQRT_3_M2     (int32_t)0x6ED8 //sqrt(3)/2
#define DIV_1_2       (int32_t)0x4000 //1/2
#define SQRT_3        (int32_t)0xDDB0 //sqrt(3)


#define TIM_ARR       3600      //3600 corrospand to 

/* Private variables ---------------------------------------------------------*/
const int16_t hsin_cos_table[256] = SIN_COS_TABLE;

/**
 * @brief  This function returns cosine and sine functions of the angle fed in
 *         input
 * @param  hangle: angle in q1.15 format
 * @retval Sin(angle) and Cos(angle) in Trig_Components format
 */
TrigComponents trig_functions(int16_t hangle)
{

  int32_t shindex;
  uint16_t uhindex;

  TrigComponents LocalComponents;

  /* 10 bit index computation  */
  shindex = ((int32_t)32768 + (int32_t)hangle);
  uhindex = (uint16_t)shindex;
  uhindex /= (uint16_t)64; // 1024

  switch ((uint16_t)(uhindex)&SIN_MASK)
  {
  case U0_90:
    LocalComponents.hsin = hsin_cos_table[(uint8_t)(uhindex)];
    LocalComponents.hcos = hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    break;

  case U90_180:
    LocalComponents.hsin = hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    LocalComponents.hcos = -hsin_cos_table[(uint8_t)(uhindex)];
    break;

  case U180_270:
    LocalComponents.hsin = -hsin_cos_table[(uint8_t)(uhindex)];
    LocalComponents.hcos = -hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    break;

  case U270_360:
    LocalComponents.hsin = -hsin_cos_table[(uint8_t)(0xFFu - (uint8_t)(uhindex))];
    LocalComponents.hcos = hsin_cos_table[(uint8_t)(uhindex)];
    break;
  default:
    break;
  }
  return (LocalComponents);
}

AlphaBeta_t rev_park_conv(Qd_t input, int16_t theta)
{
  int32_t alpha_tmp1, alpha_tmp2, beta_tmp1, beta_tmp2;
  TrigComponents local_cector_components;
  AlphaBeta_t output;

  local_cector_components = trig_functions(theta);

  /*No overflow guaranteed*/
  alpha_tmp1 = input.q * (int32_t)local_cector_components.hsin;
  alpha_tmp2 = input.d * (int32_t)local_cector_components.hcos;

  output.alpha = (int16_t)((-(alpha_tmp1) + (alpha_tmp2)) >> 15);

  beta_tmp1 = input.q * (int32_t)local_cector_components.hcos;
  beta_tmp2 = input.d * (int32_t)local_cector_components.hsin;

  output.beta = (int16_t)((beta_tmp2 + beta_tmp1) >> 15);
  return (output);
}

AlphaBeta_t clarke_conv(Ab_t input)
{
  AlphaBeta_t output;

  int32_t a_divSQRT3_tmp, b_divSQRT3_tmp;
  int32_t wbeta_tmp;
  int16_t hbeta_tmp;

  /* qIalpha = qIas*/
  output.alpha = input.a;

  a_divSQRT3_tmp = divSQRT_3 * (int32_t)input.a;

  b_divSQRT3_tmp = divSQRT_3 * (int32_t)input.b;

  wbeta_tmp = ((a_divSQRT3_tmp) + (b_divSQRT3_tmp) +
               (b_divSQRT3_tmp)) >>
              15;

  /* Check saturation of Ibeta */
  if (wbeta_tmp > INT16_MAX)
  {
    hbeta_tmp = INT16_MAX;
  }
  else if (wbeta_tmp < (-32768))
  {
    hbeta_tmp = (-32768);
  }
  else
  {
    hbeta_tmp = (int16_t)(wbeta_tmp);
  }

  output.beta = hbeta_tmp;

  if (output.beta == (int16_t)(-32768))
  {
    output.beta = -32767;
  }

  return (output);
}

Qd_t park_conv(AlphaBeta_t input, int16_t theta)
{
  Qd_t output;
  int32_t d_tmp_1, d_tmp_2, q_tmp_1, q_tmp_2;
  TrigComponents local_vector_components;
  int32_t wqd_tmp;
  int16_t hqd_tmp;
  local_vector_components = trig_functions(theta);
  /*No overflow guaranteed*/
  q_tmp_1 = input.alpha * (int32_t)local_vector_components.hsin;
  /*No overflow guaranteed*/
  q_tmp_2 = input.beta * (int32_t)local_vector_components.hcos;
  /*Iq component in Q1.15 Format */
  wqd_tmp = (-q_tmp_1 + q_tmp_2) >> 15; 
  /* Check saturation of Iq */
  if (wqd_tmp > INT16_MAX)
  {
    hqd_tmp = INT16_MAX;
  }
  else if (wqd_tmp < (-32768))
  {
    hqd_tmp = (-32768);
  }
  else
  {
    hqd_tmp = (int16_t)(wqd_tmp);
  }

  output.q = hqd_tmp;

  if (output.q == (int16_t)(-32768))
  {
    output.q = -32767;
  }

  /*No overflow guaranteed*/
  d_tmp_1 = input.alpha * (int32_t)local_vector_components.hcos;

  /*No overflow guaranteed*/
  d_tmp_2 = input.beta * (int32_t)local_vector_components.hsin;


  wqd_tmp = (d_tmp_1 + d_tmp_2) >> 15;


  /* Check saturation of Id */
  if (wqd_tmp > INT16_MAX)
  {
    hqd_tmp = INT16_MAX;
  }
  else if (wqd_tmp < (-32768))
  {
    hqd_tmp = (-32768);
  }
  else
  {
    hqd_tmp = (int16_t)(wqd_tmp);
  }

  output.d = hqd_tmp;

  if (output.d == (int16_t)(-32768))
  {
    output.d = -32767;
  }

  return (output);
}




uint8_t pwm_set_phase_voltage(AlphaBeta_t input,Uvw_t* uvw)
{


  int32_t wx,wy,wz;
  int32_t wu_alpha, wu_beta;
  int32_t tim_a, tim_b, tim_c;
  //unti-park transform
  #if 0
  uvw->u =  input.alpha;
  uvw->temp_v = -input.alpha / 2;
  uvw->temp_w = (SQRT_3_M2 * input.beta) >> 15;
  uvw->v = uvw->temp_v + uvw->temp_w;
  uvw->w = uvw->temp_v - uvw->temp_w;
  #else
  uvw->u =  input.alpha;
  // uvw->temp_v = -input.alpha / 2;
  // uvw->temp_w = (SQRT_3_M2 * input.beta) >> 15;
  uvw->v = (-input.alpha / 2) + ((SQRT_3_M2 * input.beta) >> 15);
  uvw->w = (-input.alpha / 2) - ((SQRT_3_M2 * input.beta) >> 15);
  #endif

  //sector judgment from csdn
  const uint8_t v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
  const uint8_t k_to_sector[8] = {4, 6, 5, 5, 3, 1, 2, 2};
  const uint8_t sector_trans[6] = {2, 6, 1, 4, 3, 5};
  bool A = (input.beta > 0);
  bool B = INT_ABS(input.beta) > ((SQRT_3 * INT_ABS(input.alpha)) >> 15);
  bool C = (input.alpha > 0);

  uint8_t K = 4 * A + 2 * B + C;
  uvw->k = K;
  uint8_t sector = k_to_sector[K];
  //classical section judgment
  bool new_a = (input.beta > 0);
  bool new_b = (((SQRT_3 * input.alpha) >> 15 ) - input.beta) > 0;
  bool new_c = (-(( (SQRT_3 * input.alpha) >> 15 ) + input.beta)) > 0;

  uint8_t sector_n = new_a + 2 * new_b + 4 * new_c;
  uvw->sector = sector_trans[sector_n - 1];

  //pwm value output WITH CLASSIC METION
  uint16_t pwm_time = 3600;//tim1 arr = 3600;
  uint16_t udc = 12;       // vdc = 12V
  int32_t tx, ty;
  const int32_t Ts = 65535;
  //when iq = 32767 this value max is 32767*0.9317 = 30531 min is 2236
  //when iq = 0 this value(sa,sb,sc)  is 16384
  //sa sb sc is a value centered with 16384 
  int32_t sa, sb, sc;
  switch (uvw->sector)
  {
  case 1:
    tx =  (SQRT_3*(((SQRT_3_M2*input.alpha)>>15) - (input.beta / 2)))>>15;
    ty =  (SQRT_3*input.beta)>>15;
    sa =  (Ts - tx - ty) / 4;
    sb =  (Ts + tx - ty) / 4;
    sc =  (Ts + tx + ty) / 4;
    break;
  case 2:
    tx =  (SQRT_3* ( (-((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty =  (SQRT_3* ( ( ((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx - ty) / 4;
    sb =  (Ts - tx - ty) / 4;
    sc =  (Ts + tx + ty) / 4;
    break;
  case 3:
    tx =  (SQRT_3*input.beta)>>15;
    ty = -(SQRT_3* ( ( ((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx + ty) / 4;
    sb =  (Ts - tx - ty) / 4;
    sc =  (Ts + tx - ty) / 4;
    break;
  case 4:
    tx = -(SQRT_3*input.beta)>>15;
    ty =  (SQRT_3* ( (-((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx + ty) / 4;
    sb =  (Ts + tx - ty) / 4;
    sc =  (Ts - tx - ty) / 4;
    break;
  case 5:
    tx = -(SQRT_3* ( ( ((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty = -(SQRT_3* ( (-((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx - ty) / 4;
    sb =  (Ts + tx + ty) / 4;
    sc =  (Ts - tx - ty) / 4;
    break;
  case 6:
    tx =  (SQRT_3* ( ( ((SQRT_3_M2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty = -(SQRT_3*input.beta)>>15;
    sa =  (Ts - tx - ty) / 4;
    sb =  (Ts + tx + ty) / 4;
    sc =  (Ts + tx - ty) / 4;
    break;
  default:
    break;
  }

  uvw->tx = tx;
  uvw->ty = ty;
  uvw->sa = sa;
  uvw->sb = sb;
  uvw->sc = sc;
  //tim arr: 3600   sa sb sc central value is 16384
  //2^14 = 16384
  uvw->ta = 1800 - (((sa - 16384) * 1800)>>(15 - 1));
  uvw->tb = 1800 - (((sb - 16384) * 1800)>>(15 - 1));
  uvw->tc = 1800 - (((sc - 16384) * 1800)>>(15 - 1));
  return sector;
}

int main(void)
{
#if 1
  MOTOR motor = {0};
  AlphaBeta_t alpha_beta = {0};
  TrigComponents trig = {0};
  Qd_t qd = {0};
  Uvw_t uvw = {0};
  motor.motor_func.trig_functions = trig_functions;
  motor.ctl_param.id_ref = 0;
  motor.ctl_param.iq_ref = 1024;
  qd.d = 0;
  qd.q = -32767;//max current is 32767
  int32_t theta;
  uint8_t sector;
  for(uint16_t usr_theta = 0; usr_theta < 65535; usr_theta++)
  {
    theta = usr_theta;
    if(usr_theta > 32767)
    {
      theta -= 65535;
    }
    alpha_beta = rev_park_conv(qd, theta);
    sector = pwm_set_phase_voltage(alpha_beta, &uvw);
    if(theta%182 == 0)
    {
      printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",\
      theta, alpha_beta.alpha, alpha_beta.beta, \
      sector, uvw.k, uvw.sector, uvw.tx, uvw.ty, uvw.sa, uvw.sb, uvw.sc, uvw.ta, uvw.tb, uvw.tc);
    }
    
  }

#else
  float id = 0.0f;
  float iq = 32765.0f;
  const float usr_pi = 3.1415; 
  float cos_value, sin_value, ia, ib, iu, iv, iw;
  for(uint16_t i = 0; i < 360; i++)
  {
    //unpark
    cos_value = cosf(i / 360.0 * 2.0 * usr_pi);
    sin_value = sinf(i / 360.0 * 2.0 * usr_pi);
    ia = id * cos_value - iq * sin_value;
    ib = id * sin_value + iq * cos_value;
    //unclark
    iu = ia;
    iv = -ia / 2 + 1.732f * ib / 2;
    iw = -ia / 2 - 1.732f * ib / 2;
    //sector judgement
    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2};
    bool a = ib > 0;
    bool b = fabs(ib) > 1.732f * fabs(ia);
    bool c = ia > 0;

    int k = 4 * a + 2 * b + c;
    int sector = K_to_sector[k];
    printf("%f, %f, %f, %f, %f, %d\r\n",ia, ib, iu, iv, iw, sector); 

  }

#endif
  return 0;
}